/*
    订阅turtle1 和 turtle2 的 TF广播信息， 查找并转换时间最近的TF信息
    将turtle1 转换过成先谷底 turtle2 的做标牌 在计算线速度和角速度并发布





*/


#include<ros/ros.h>
#include<tf2_ros/buffer.h>
#include<tf2_ros/transform_listener.h>
#include<geometry_msgs/Twist.h>
#include<geometry_msgs/TransformStamped.h>

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");

    ros::init(argc, argv, "sub_TF");

    ros::NodeHandle nh;

    tf2_ros::Buffer buffer;

    tf2_ros::TransformListener listener(buffer);


    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",100);

    ros::Rate rate(10);


    while (ros::ok())
    {
        try
        {
            geometry_msgs::TransformStamped tfs = buffer.lookupTransform("turtle2","turtle1",ros::Time(0.0));
            geometry_msgs::Twist twist;

            twist.linear.x = 0.5 *sqrt(pow(tfs.transform.translation.x,2)+pow(tfs.transform.translation.y,2));
            twist.angular.z = 4*atan2(tfs.transform.translation.y,tfs.transform.translation.x);
        }
        catch(const std::exception& e)
        {
            std::cerr << e.what() << '\n';
        }

        rate.sleep();
        ros::spinOnce();
            
    }
    
    


    return 0;
}
